#include "FLAlgorithm.hpp"

FLAlgorithm::FLAlgorithm(vector< vector<string> > *algM): SwarmAlgorithm() {
    id = "FOLLOWLEADER";
    algMessages = algM;
}

string FLAlgorithm::readSwarmInst() {
    while (algMessages->empty()) {
        SwarmOS::refreshNetworkMessages();
    }
    string retVal = algMessages->front()[0];
    algMessages->erase(algMessages->begin());
    return retVal;
}

void FLAlgorithm::leaderQuery()
{
    int id;
    double d;
    string mess;
    mess = SwarmOS::AL_INSTRUCTION + itoa(SwarmOS::getNetworkID()) + " DISTFROMLEADER";
    for(int i = 1; i < SwarmOS::getSwarmSize(); i++)
    {
        id = -1;
        d = -1.0;
        if(SwarmOS::getIsConnected(i) && !inTrain[i])
        {
            
            while(true)
            {
                SwarmOS::writeMessage(i, mess);
                string inst = readSwarmInst();
                if(sscanf(inst.c_str(), "%i %lf", &id, &d) == 2)
                {
                    if(id == i && d > 0)
                    {
                        dist[i] = d;
                        break;
                    }
                }
                /*if(id == i && d > 0)
                {
                    break;
                }*/
            }
        }
        else
        {
            dist[i] = DBL_MAX;
        }
    }
}



//void FLAlgorithm::leaderDesignate();
void FLAlgorithm::leaderDesignate()
{
    int id = -1;
    double d = DBL_MAX;
    map<double, int> shortest;
    string mess;
    mess = SwarmOS::AL_INSTRUCTION + itoa(SwarmOS::getNetworkID()) + " YOUARELEADER";
    for(int i = 1; i <= dist.size(); i++)
    {
        if(SwarmOS::getIsConnected(i) && !inTrain[i])
        {
            shortest[dist[i]] = i;
        }
    }
    for(map<double, int>::iterator it = shortest.begin(); it != shortest.end(); ++it)
    {
        if(SwarmOS::writeMessage(it->second, mess) == mess.size())
        {
            myFollower = it->second;
            inTrain[it->second] = true;
            mess = SwarmOS::AL_INSTRUCTION + itoa(it->second) + " IAMFOLLOWER";
            SwarmOS::broadcast(mess);
            break;
        }
    }
    //mess = itoa(botnum);
    //mess += " trainIsComplete";
    //netWrapper.client.writeMessage(mess.c_str(), mess.size());
}
void FLAlgorithm::leaderWaitForReady()
{
    char cs[50];
    string s;
    int id;
    while(true)
    {
        string inst = readSwarmInst();
        if(sscanf(inst.c_str(), "%i %s", &id, cs) == 2)
        {
            s = cs;
            if(id == myFollower && s == "IAMREADY")
            {
                break;
            }
        }
    }
}
void FLAlgorithm::leaderWaitForTrain()
{
    char cs[50];
    string s;
    int id;
    while(true)
    {
        string inst = readSwarmInst();
        if(sscanf(inst.c_str(), "%i %s", &id, cs) == 2)
        {
            s = cs;
            if(s == "TRAINISCOMPLETE")//if(id == 0 && s == "trainIsComplete")
            {
                break;
            }
        }
    }
}


void FLAlgorithm::followerWaitForInstruction()
{
    char cs[50];
    string mess, s;
    int id;
    std::vector<Robot> robots;
    while(true)
    {
        string inst = readSwarmInst();
        if(sscanf(inst.c_str(), "%i %s", &id, cs) == 2)
        {
            s = cs;
            if(s == "DISTFROMLEADER")
            {
                robots = robotFinder->getRobotPositions();
                mess = SwarmOS::AL_INSTRUCTION + itoa(SwarmOS::getNetworkID()) + " ";
                if(robots.size() > 0)
                {
                    mess += ftoa(robots[0].position.radius_cm);
                }
                else
                {
                    mess += ftoa(DBL_MAX);
                }
                SwarmOS::writeMessage(id, mess);
            }
            else if(s == "YOUARELEADER")
            {
                inTrain[SwarmOS::getNetworkID()] = true;
                myLeader = id;
                break;
            }
            else if(s == "IAMFOLLOWER")
            {
                inTrain[id] = true;
            }
        }
    }
}
void FLAlgorithm::followerGetReady()
{
    double offset = 3.14/16;
    //double rad, angle;
    string mess;
    std::vector<Robot> robots = robotFinder->getRobotPositions();
    Motor motor;
    while(true)
    {

        robots = robotFinder->getRobotPositions();
        //cout << robots.size() << " ";
        rad = -1;
        for(int i = 0; i < robots.size(); i++)
        {
            if((rad < 0 || robots[i].position.radius_cm < rad) && color == LED::colorStringToInt(robots[i].label))
            {
            	rad   = robots[i].position.radius_cm;
            	angle = robots[i].position.angle_rad;
            }
        }
        if(rad > 0)//(robots.size() > 0)
        {
            //cout << "Rad " << rad << " Angle " << angle << endl;
            if(angle > offset)
            {
            //cout << "Crawl left\n";
            motor.leftWheelReverseOffset(5);
            motor.rightWheelGoOffset(5);
            }
            else if(angle < -offset)
            {
            //cout << "Crawl right\n";
            motor.rightWheelReverseOffset(5);
            motor.leftWheelGoOffset(5);
            }
            else // if(angle > -offset && angle < offset)
            {
            //cout << "Crawl forward\n";
            //motor.goOffset(5);
            mess = SwarmOS::AL_INSTRUCTION + itoa(SwarmOS::getNetworkID()) + " IAMREADY";
            SwarmOS::writeMessage(myLeader, mess);
            break;
            }

//            struct timespec tim, tim2;
  //          tim.tv_sec = 0;
    //        tim.tv_nsec = 500000000;

    //        nanosleep(&tim , &tim2);
    //        motor.stop();
    //        nanosleep(&tim , &tim2);
        }
        else {
            motor.rightWheelReverseOffset(5);
            motor.leftWheelGoOffset(5);
        }
    }
}
void FLAlgorithm::followerBecomeMiniLeader()
{
    string mess;
    bool trainIsComplete = true;
    led->setLED(true);
    for(int i = 1; i < inTrain.size(); i++)
    {
        if(!inTrain[i] && SwarmOS::getIsConnected(i))
        {
            trainIsComplete = false;
            break;
        }
    }
    if(!trainIsComplete)
    {
        leaderQuery();
        leaderDesignate();
        leaderWaitForReady();
    }
    else
    {
        mess = SwarmOS::AL_INSTRUCTION + itoa(SwarmOS::getNetworkID()) + " TRAINISCOMPLETE";
        SwarmOS::writeMessage(SwarmOS::HOST_ID, mess);
    }
        
}

bool FLAlgorithm::setup() {
    inTrain = vector<bool>(SwarmOS::getSwarmSize(), false);
    string mess;

    string inst = readSwarmInst();
    if (inst == "RED") {
        color = LED::RED;
        led = red;
    }
    else if (inst == "GREEN") {
        color = LED::GREEN;
        led = green;
    }
    else if (inst == "BLUE") {
        color = LED::BLUE;
        led = blue;
    }
	
    inst = readSwarmInst();
    isLeader = SwarmOS::getNetworkID() == atoi(inst.c_str());
    bool trainIsComplete = false;

    if(isLeader)
    {
        led->setLED(true);
        inTrain[SwarmOS::getNetworkID()] = true;
        trainIsComplete = true;
        for(int i = 1; i < inTrain.size(); i++)
        {
            if(!inTrain[i] && SwarmOS::getIsConnected(i))
            {
                trainIsComplete = false;
                break;
            }
        }
        if(!trainIsComplete)
        {
            leaderQuery();
            leaderDesignate();
            leaderWaitForReady();
			led->setLED(false);
        	leaderWaitForTrain();
        	led->setLED(true);
        }
        else
        {
            mess = SwarmOS::AL_INSTRUCTION + itoa(SwarmOS::getNetworkID()) + " TRAINISCOMPLETE";
            SwarmOS::writeMessage(SwarmOS::HOST_ID, mess);
            //todo: broadcast train complete
			SwarmOS::broadcast(mess);
        }
        
    }
    else
    {
        followerWaitForInstruction();
        followerGetReady();
        followerBecomeMiniLeader();
        led->setLED(false);
        leaderWaitForTrain();
        led->setLED(true);
    }

    return true;
}

void FLAlgorithm::loopBody() {
    if(!isRunning)
    {
        motorControl->stop();
        led->setLED(false);
        return;
    }
    if(isLeader)
    {
        isRunning = false;
        return;
    }
    else
    {
        clusterTrack(*robotFinder, rad, angle, *motorControl, *bumperControl, color); // todo: make sure this works
    }
}
